Program Listing for File map_add.h

↰ Return to documentation for file (codes/cubicle_merge/map_add.h)

/*******************************************************
 * Copyright (C) 2019, Robotics Group, Nanyang Technology University
 *
 * \file map_add.h
 * \author Zhang Handuo (hzhang032@e.ntu.edu.sg)
 * \date Januarary 2017
 * \brief Add incoming obstacle_msgs into database and publish them.
 *
 * Licensed under the GNU General Public License v3.0;
 * you may not use this file except in compliance with the License.
 *
 *******************************************************/

#ifndef PROJECT_MAP_ADD_H
#define PROJECT_MAP_ADD_H
#include <iostream>
#include <sstream>
#include <cstring>
#include <fstream>
#include <cmath>
#include <ctime>
// Thirdparty
#include <Eigen/Dense>
#include <Eigen/Geometry>
// ROS
#include <ros/ros.h>
#include <tf_conversions/tf_eigen.h>
//#include <std_msgs/String.h>
//#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_listener.h>
// Custom
#include "obstacle_ind_merge.h"
#include "map_publisher.h"
#include "obstacle_merge.h"
#include <obstacle_msgs/MapInfo.h>
#include <obstacle_msgs/obs.h>
//#include <obstacle_msgs/point3.h>
#include "util.h"
//#include "tf_utils.hpp"
//#include "tracked_obstacle.h"

class obstacle_ind_merge;
class Obstacle_merge;
class MapPublisher;

class MapAdd {
public:

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#endif /* DOXYGEN_SHOULD_SKIP_THIS */

    MapAdd( const ros::NodeHandle& nh, obstacle_ind_merge *pObstacle_ind_merge,
            Obstacle_merge *pObstacle_merge, MapPublisher *pMapPublisher, int input_source, bool local_map);

    ~MapAdd();

    void addMapSingle(const obstacle_msgs::MapInfo::ConstPtr& );

    void pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr&);

    void obstacle_map_callback( const obstacle_msgs::MapInfo::ConstPtr& );

    void lane_callback( const obstacle_msgs::MapInfo::ConstPtr& );

    void curb_callback( const obstacle_msgs::MapInfo::ConstPtr& );
    double smooth_thres;
private:

    void transformTFToEigen(
            const tf::Transform& tf_type,
            Eigen::Isometry3d& pose_) {
        Eigen::Vector3d position;
        Eigen::Quaterniond rotation;

        tf::quaternionTFToEigen(tf_type.getRotation(), rotation);
        tf::vectorTFToEigen(tf_type.getOrigin(), position);

        // Enforce positive w.
        if (rotation.w() < 0) {
            rotation.coeffs() = -rotation.coeffs();
        }

        pose_.translation() = position;
        pose_.linear() = rotation.toRotationMatrix();
    }

    bool lookupTransformTf(const ros::Time& time, Eigen::Isometry3d& cameraPose);

    tf::TransformListener tf_listener_;

    std::string world_frame_;

    std::string ego_frame_;

    int input_source_;

    bool local_map_;
    bool tf_exist_, initialized_;

    Eigen::Isometry3d current_pose;

    double pose_timestamp_;

    // obstacle individual merge
    obstacle_ind_merge *mpObstacle_ind_merge;

    // obstacles merge
    Obstacle_merge *mpObstacle_merge;

    MapPublisher *mpMapPublisher;

    std::string map_frame_id;

};

#endif //PROJECT_MAP_ADD_H